/***************************************
 *
 *     与内核交互
 *
 **************************************/

#include "user_operation.h"

#include <Windows.h>
#include <chrono>
#include <ctime>

// 全局变量
constexpr char customer_number_[] = "070075"; // 客户号，需要配合加密狗共同使用
const char* slave_dc_name = "DC";
DemoShareData* app_shared_data_ = nullptr; // 应用共享内存数据
DemoShareData* kernel_share_data_ = nullptr; // 内核共享内存数据
KSHandle adapter_handle_ = NULL; // 全局网络适配器
KSHandle kernel_handle_ = NULL; // 全局内核操作句柄
KSHandle data_set_call_back_ = NULL; // 数据集的回调
KSHandle topology_call_back_ = NULL; // 数据集的回调
KSHandle timer_call_back_ = NULL; // 计时器回调
KSHandle timer_handle_ = NULL; // 全局计时器
#define  CONTROL_PERIOD 1000                   // 控制周期 单位us
#define  CONTROL_MODE 0x08                       //控制模式 csp 周期同步位置模式


#pragma pack(push, 1)
struct Data {
    unsigned char data[2501];
};

struct WriteData {
    uint32_t data[128];
};
#pragma pack(pop)
static Data w_data{
    {
        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30,
        31, 32, 33, 34
    }
};
static int w_size = 2001;

// 错误码转错误描述
void OutPutError(const std::string& err_description, const KSError& err) {
    const char* msg;
    KS_getErrorString(err, &msg, 0);
    printf("%s ,0x%X: %s\n", err_description.c_str(), err, msg);
}

KSError GetKitharaSystemInfo(KitharaSystemInfo& info) {
    // 1. 打开驱动
    KSError error = KS_openDriver(customer_number_);
    if (error != KS_OK) {
        OutPutError("open device failed", error);
        KS_closeDriver();
        return error;
    }

    // 2. 获取CPU信息
    KSSystemInformation system_info;
    system_info.structSize = sizeof(KSSystemInformation); // 特别注意要分配大小
    error = KS_getSystemInformation(&system_info, 0);

    if (error != KS_OK) {
        OutPutError("GetCPUInfo Failed!", error);
        KS_closeDriver();
        return error;
    }

    info.total_cpu_count = system_info.numberOfCPUs;
    info.shared_cpu_count = system_info.numberOfSharedCPUs;

    // 3. 遍历网络设备
    for (int i = 0;; ++i) {
        char device_name[256]; // 设备名
        error = KS_enumDevices("NET", i, device_name, 0);

        if (error != KS_OK) { break; }

        if ((error != KS_OK) && (i == 0)) {
            OutPutError("NetWork Not Fund!", error);
            KS_closeDriver();
            return error;
        }

        printf("%d: %s \n", i, device_name);
        info.net_work_list.append(device_name);
    }

    return KS_OK;
}

KSError StartRunning(const int processor, const int index, const std::string& file_path) {
    // 设置CPU
    KSError error = KS_setTargetProcessor(processor, 0);
    if (error != KS_OK) {
        OutPutError("KS_setTargetProcessor KSError!", error);
        KS_closeDriver();
        return error;
    }

    // 4.选择网卡
    char device_name[256];
    error = KS_enumDevices("NET", index, device_name, 0);

    if (error != KS_OK) {
        OutPutError("no network device found", error);
        KS_closeDriver();
        return error;
    }

    printf("Selected device: %s \n", device_name);

    // 打开适配器
    error = KS_openAdapter(&adapter_handle_, device_name, 256, 256, KSF_REALTIME_EXEC);

    if (error != KS_OK) {
        OutPutError("KS_openAdapter", error);
        return error;
    }

    // 创建共享内存,加载内核DLL前需要先创建共享内存
    error = KS_createSharedMem(
        reinterpret_cast<void**>(&app_shared_data_), reinterpret_cast<void**>(&kernel_share_data_), NULL,
        sizeof(DemoShareData), 0);
    if (error != KS_OK) {
        OutPutError("KS_createSharedMem failed", error);
        return error;
    }

    // 5. 加载实时层Dll
    error = KS_loadKernel(&kernel_handle_, "Kernel.dll", NULL, NULL, KSF_KERNEL_EXEC | KSF_SAVE_FPU);

    if (error != KS_OK) {
        OutPutError("KS_loadKernel", error);
        return error;
    }

    // 初始化内核层
    error = KS_execKernelFunction(
        kernel_handle_, "InitFunction", kernel_share_data_, NULL, KSF_KERNEL_EXEC | KSF_SAVE_FPU);
    if (error != KS_OK) {
        OutPutError("KS_execKernelFunction InitFunction", error);
        return error;
    }

    // 6. 创建主站
    error = KS_createEcatMaster(&app_shared_data_->master_handle, adapter_handle_, file_path.c_str(), "", 0);
    if (error != KS_OK) {
        OutPutError("KS_createEcatMaster Failed", error);
        return error;
    }

    // 获取主站状态
    KSEcatMasterState master_stater;
    master_stater.structSize = sizeof(KSEcatMasterState); // 特别注意设置结构体大小

    for (int i = 0; i < 100; ++i) {
        error = KS_queryEcatMasterState(app_shared_data_->master_handle, &master_stater, 0);

        if (error != KS_OK) {
            OutPutError("KS_queryEcatMasterState Failed", error);
            return error;
        }

        if (master_stater.connected) { break; }
    }

    if (!master_stater.connected) {
        OutPutError("MasterState not connected", error);
        return error;
    }

    // 输出从站个数
    printf("slavesOnline = %d\n", master_stater.slavesOnline);

    // 校验扫描从站个数和实际个数
    if (master_stater.slavesOnline != SLAVE_COUNT) {
        OutPutError("slaves count not match", error);
        return 1;
    }

    // 更改主站状态
    error = KS_changeEcatMasterState(app_shared_data_->master_handle, KS_ECAT_STATE_INIT, 0);
    if (error != KS_OK) {
        OutPutError("KS_changeEcatMasterState Failed", error);
        return error;
    }

    // 网络拓扑结构发送改变后操作（从站热拔插）
    error = KS_createKernelCallBack(
        &topology_call_back_, kernel_handle_, "TopologyCallBack", nullptr, KSF_DIRECT_EXEC | KSF_SAVE_FPU, 0);
    if (error != KS_OK) {
        OutPutError("KS_createKernelCallBack DataSetCallBack  Failed", error);
        return error;
    }

    error = KS_installEcatHandler(app_shared_data_->master_handle, KS_TOPOLOGY_CHANGE, topology_call_back_, 0);
    if (error != KS_OK) {
        OutPutError("KS_installEcatHandler KS_TOPOLOGY_CHANGE Failed", error);
        return error;
    }

    // PDO数据反馈的回调函数
    error = KS_createKernelCallBack(
        &data_set_call_back_, kernel_handle_, "DataSetCallBack", nullptr, KSF_DIRECT_EXEC | KSF_SAVE_FPU, 0);
    if (error != KS_OK) {
        OutPutError("KS_createKernelCallBack DataSetCallBack  Failed", error);
        return error;
    }

    // 创建DataSet数据集
    error = KS_createEcatDataSet(
        app_shared_data_->master_handle, &app_shared_data_->data_set_handle, nullptr, nullptr, 0);
    if (error != KS_OK) {
        OutPutError("KS_createEcatDataSet  Failed", error);
        return error;
    }

    // 安装回调函数
    error = KS_installEcatHandler(app_shared_data_->data_set_handle, KS_DATASET_SIGNAL, data_set_call_back_, 0);
    if (error != KS_OK) {
        OutPutError("KS_installEcatHandler  Failed", error);
        return error;
    }

    // 创建计时器，创建计时器回调
    error = KS_createKernelCallBack(
        &timer_call_back_, kernel_handle_, "TimerCallBack", nullptr, KSF_DIRECT_EXEC | KSF_SAVE_FPU, 0);
    if (error != KS_OK) {
        OutPutError("create timerCallBack failed", error);
        return error;
    }

    error = KS_createTimer(
        &timer_handle_, CONTROL_PERIOD * 10, timer_call_back_,
        KSF_DIRECT_EXEC | KSF_DONT_START); // 注意：若使用同步时钟，则不能直接启动定时器。

    if (error != KS_OK) {
        OutPutError("KS_createTimer KSError", error);
        return error;
    }
    // 从站信息
    KSEcatSlaveState state;
    state.structSize = sizeof(KSEcatSlaveState);
    // DC配置局部变量
    KSEcatDcParams params;

    // 创建并配置所有从站
    for (int i = 0; i < SLAVE_COUNT; i++) {
        // 枚举从站
        KS_enumEcatSlaves(app_shared_data_->master_handle, i, &state, 0);
        printf("Slave %d vendor: 0x%X \n", i, state.vendor);
        printf("Slave %d product: 0x%X \n", i, state.product);
        printf("Slave %d slaveState: 0x%X \n", i, state.slaveState);

        error = KS_createEcatSlave(
            app_shared_data_->master_handle, &app_shared_data_->slave_handles[i], 0, i, 0, 0, 0, KSF_FORCE_OVERRIDE);
        if (error != KS_OK) {
            OutPutError("KS_createEcatSlave Failed", error);
            return error;
        }

        // 修改从站状态
        error = KS_changeEcatState(app_shared_data_->slave_handles[i], KS_ECAT_STATE_PREOP, 0);
        if (error != KS_OK) {
            OutPutError("KS_changeEcatState Failed", error);
            return error;
        }

        // 分配从站POD数据到数据集
        error = KS_assignEcatDataSet(
            app_shared_data_->data_set_handle, app_shared_data_->slave_handles[i], KS_ECAT_SYNC_ALL, 0, 0);
        if (error != KS_OK) {
            OutPutError("KS_assignEcatDataSet Failed", error);
            return error;
        }

        // 配置起始输入输出通道,获取从站PDO接口
        if (0xb6 == state.product) // IO 设备
        {
            // 分配从站POD数据到数据集
            error = KS_assignEcatDataSet(
                app_shared_data_->data_set_handle, app_shared_data_->slave_handles[i], KS_ECAT_SYNC_ALL, 0, 0);
            if (error != KS_OK) {
                OutPutError("KS_assignEcatDataSet Failed", error);
                return error;
            }

            // 根据XML文件获取输入输出通道
            error = KS_getEcatDataObjAddress(
                app_shared_data_->data_set_handle, app_shared_data_->slave_handles[i], 0x1A00, 0,
                (void**)&app_shared_data_->io_app_pod.IO_value_in, (void**)&app_shared_data_->io_system_pod.IO_value_in,
                0, 0, 0);

            if (error != KS_OK) {
                OutPutError("KS_getEcatDataObjAddress Failed", error);
                return error;
            }

            error = KS_getEcatDataObjAddress(
                app_shared_data_->data_set_handle, app_shared_data_->slave_handles[i], 0x1600, 0,
                (void**)&app_shared_data_->io_app_pod.IO_value_out,
                (void**)&app_shared_data_->io_system_pod.IO_value_out, 0, 0, 0);
            if (error != KS_OK) {
                OutPutError("KS_getEcatDataObjAddress Failed", error);
                return error;
            }
        }

        if (0x60380004 == state.product) // 伺服器
        {
            // 重新配置输入通道映射
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1600, -1, 0, 0, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }

            //  0x6040 ：Controlword
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1600, 0x6040, 0, 16, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }

            // 0x6060 ：Modes of operation
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1600, 0x6060, 0, 8, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }

            // 0x607A ：Target position
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1600, 0x607A, 0, 32, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }

            error = KS_setEcatPdoAssign(app_shared_data_->slave_handles[i], KS_ECAT_SYNC_OUTPUT, -1, 0);
            if (error != KS_OK) {
                OutPutError("KS_setEcatPdoAssign error", error);
                return error;
            }
            error = KS_setEcatPdoAssign(app_shared_data_->slave_handles[i], KS_ECAT_SYNC_OUTPUT, 0x1600, 0);
            if (error != KS_OK) {
                OutPutError("KS_setEcatPdoAssign error", error);
                return error;
            }
            // 重新配置输出通道映射
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1A00, -1, 0, 0, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }
            // 0x6041 ：Statusword
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1A00, 0x6041, 0, 16, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }
            // 0x6061 ： Modes of operation display
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1A00, 0x6061, 0, 8, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }

            // 0x6064 ：Position actual value
            error = KS_setEcatPdoMapping(app_shared_data_->slave_handles[i], 0x1A00, 0x6064, 0, 32, 0);
            if (error != KS_OK) {
                OutPutError("EKS_setEcatPdoMapping error", error);
                return error;
            }

            error = KS_setEcatPdoAssign(app_shared_data_->slave_handles[i], KS_ECAT_SYNC_INPUT, -1, 0);
            if (error != KS_OK) {
                OutPutError("KS_setEcatPdoAssign error", error);
                return error;
            }
            error = KS_setEcatPdoAssign(app_shared_data_->slave_handles[i], KS_ECAT_SYNC_INPUT, 0x1A00, 0);
            if (error != KS_OK) {
                OutPutError("KS_setEcatPdoAssign error", error);
                return error;
            }

            // 分配从站POD数据到数据集
            error = KS_assignEcatDataSet(
                app_shared_data_->data_set_handle, app_shared_data_->slave_handles[i], KS_ECAT_SYNC_ALL, 0, 0);
            if (error != KS_OK) {
                OutPutError("KS_assignEcatDataSet Failed", error);
                return error;
            }

            // 配置DC时钟 DC：根据XML文件获取名称，应可配置
            error = KS_lookupEcatDcOpMode(app_shared_data_->slave_handles[i], slave_dc_name, &params, 0);
            if (error != KS_OK) {
                OutPutError("KS_lookupEcatDcOpMode Failed", error);
                return error;
            }

            error = KS_configEcatDcOpMode(app_shared_data_->slave_handles[i], slave_dc_name, &params, 0);
            if (error != KS_OK) {
                OutPutError("KS_configEcatDcOpMode Failed", error);
                return error;
            }
        }
    }

    printf("CreateMasterAndSlave Success \n");

    // 启动定时器
    int64 startTime = 0;
    uint cycleTime = CONTROL_PERIOD * 1000;
    int shiftTime = 0;
    //17、启动定时器
    error = KS_activateEcatDcMode(app_shared_data_->data_set_handle, startTime, cycleTime, shiftTime, timer_handle_, 0);
    if (error != KS_OK) {
        OutPutError("KS_activateEcatDcMode Error", error);
        return error;
    }

    // 修改主站状态
    error = KS_changeEcatState(app_shared_data_->master_handle, KS_ECAT_STATE_SAFEOP, 0);
    if (error != KS_OK) {
        OutPutError("KS_changeEcatState Master  Failed", error);
        return error;
    }

    // 修改数据集状态
    error = KS_changeEcatState(app_shared_data_->data_set_handle, KS_ECAT_STATE_OP, 0);
    if (error != KS_OK) {
        OutPutError("KS_changeEcatState  DataSet  Failed", error);
        return error;
    }

    for (int i = 0; i < SLAVE_COUNT; i++) {
        error = KS_changeEcatState(app_shared_data_->slave_handles[i], KS_ECAT_STATE_OP, 0);
        if (error != KS_OK) {
            OutPutError("KS_changeEcatState to SAFEOP Error", error);
            return error;
        } else { printf("switch to  OP \n"); }
    }

    OutPutError("start work  ... ", error);
    app_shared_data_->EcatState_OP = true;
    return error;
}

void GetPDOData(DemoPODIOData& io_pdo_data) {
    if (app_shared_data_ != nullptr) {
        io_pdo_data = app_shared_data_->io_app_pod;
        //printf("\r --------- Input:0x%X,Output:0x%X  \n", *app_share_data_->io_app_pod.IO_value_in, *app_share_data_->io_app_pod.IO_value_out);
    }
}

void SetOutPutValue(int value_out) {
    if (app_shared_data_ != NULL) { *app_shared_data_->io_app_pod.IO_value_out = value_out; }
}

void ExitApp() {
    if (app_shared_data_ != NULL && app_shared_data_->master_handle != NULL) {
        // 切换主站状态
        KS_changeEcatState(app_shared_data_->master_handle, KS_ECAT_STATE_INIT, 0);
    }

    if (timer_handle_ != NULL) {
        KS_stopTimer(timer_handle_);
        KS_removeTimer(timer_handle_);
    }
    if (timer_call_back_ != NULL) { KS_removeCallBack(timer_call_back_); }

    if (app_shared_data_ != NULL && app_shared_data_->data_set_handle != NULL) {
        // 删除数据集
        KS_deleteEcatDataSet(app_shared_data_->data_set_handle);
    }

    // 移除所有从站
    if (app_shared_data_ != NULL) {
        for (const auto slave : app_shared_data_->slave_handles) { KS_deleteEcatSlave(slave); }
    }

    // 取消安装的回调
    if (app_shared_data_ != NULL && app_shared_data_->master_handle != NULL) {
        KS_installEcatHandler(app_shared_data_->master_handle, KS_TOPOLOGY_CHANGE, NULL, 0);
    }

    // 关闭主站
    if (app_shared_data_ != NULL && app_shared_data_->master_handle != NULL) {
        KS_closeEcatMaster(app_shared_data_->master_handle);
    }

    // 移除数据集的回调
    if (data_set_call_back_ != NULL) { KS_removeCallBack(data_set_call_back_); }

    // 释放内核
    if (kernel_handle_ != NULL) { KS_freeKernel(kernel_handle_); }

    // 释放共享内存
    if (app_shared_data_ != NULL) { KS_freeSharedMem(app_shared_data_); }

    // 关闭适配器
    if (adapter_handle_ != NULL) { KS_closeAdapter(adapter_handle_); }

    // 关闭设备
    KS_closeDriver();

    Sleep(1000);
}

KSError SDO_Download(const int obj_index, void* data, const int size) {
    for (const int slave_handle : app_shared_data_->slave_handles) {
        if (slave_handle != NULL) {

            uint32_t sdo_value[200]{};
            for (int i = 0; i < 200; i++) {
                sdo_value[i] = i;
            }
            auto start_time = std::chrono::high_resolution_clock::now();
            clock_t start = clock(); // 开始计时

            KSError error = KS_postEcatDataObj(app_shared_data_->slave_handles[0], 0x4001, 1, &sdo_value, sizeof(sdo_value), KSF_SDO | KSF_COMPLETE_ACCESS);

            clock_t end = clock(); // 结束计时
            // 记录结束时间
            auto end_time = std::chrono::high_resolution_clock::now();

            // 计算执行时间
            auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time).count();

            // 计算时间（单位：秒）
            double duration_clock = static_cast<double>(end - start) / CLOCKS_PER_SEC;
            if (error != KS_OK) {
                OutPutError("postEcatSDOData ctrlMode Error", error);
                return error;
            }
            // 输出执行时间
            printf("KS_postEcatDataObj Execution Time: %lld microseconds\n", duration);

            std::cout << "Execution time:" << duration_clock << " seconds" << std::endl;

            //
            // WriteData write_sdo_data{};
            // int i = 0;
            // for (auto& item : write_sdo_data.data) {
            //     item = i;
            //     i++;
            // }
            // start_time = std::chrono::high_resolution_clock::now();
            // start = clock(); // 开始计时
            //
            // error = KS_postEcatDataObj(app_shared_data_->slave_handles[0], 0x4001, 1, &write_sdo_data, sizeof(WriteData), KSF_SDO | KSF_COMPLETE_ACCESS);
            //
            // end = clock(); // 结束计时
            // // 记录结束时间
            // end_time = std::chrono::high_resolution_clock::now();
            //
            // // 计算执行时间
            // duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time).count();
            //
            // // 计算时间（单位：秒）
            // duration_clock = static_cast<double>(end - start) / CLOCKS_PER_SEC;
            // if (error != KS_OK) {
            //     OutPutError("0x40A0 postEcatSDOData ctrlMode Error", error);
            //     return error;
            // }
            // // 输出执行时间
            // printf("KS_postEcatDataObj 0x40A0 Execution Time: %lld microseconds\n", duration);
            //
            // std::cout << "KS_postEcatDataObj 0x40A0 Execution time:" << duration_clock << " seconds" << std::endl;

        }
    }
    printf("SDO_Download Success \n");
    return KS_OK;
}

KSError SDO_GetData(const int obj_index, void* data, int& size) {
#pragma pack(push, 1)
    struct ReadData {
        unsigned char data[2501];
    };
#pragma pack(pop)
    ReadData read_data{};
    WriteData read_test{};
    uint32_t read_sdo_data[200]{};
    int data_size = sizeof(read_sdo_data);
    // 读取SDO值并打印
    for (const int slave_handle : app_shared_data_->slave_handles) {
        if (slave_handle != NULL) {
            // // 记录开始时间
            // auto start_time = std::chrono::high_resolution_clock::now();
            // KSError error = KS_readEcatDataObj(slave_handle, 0x4001, 0, &read_sdo_data, &data_size, KSF_SDO);
            // // 记录结束时间
            // auto end_time = std::chrono::high_resolution_clock::now();
            // // 计算执行时间
            // auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time).count();
            // // 输出执行时间
            // printf("read 0x40A0 ,length: %d \n", data_size);
            // printf("KS_readEcatDataObj Execution Time: %lld microseconds\n", duration);
            // if (error != KS_OK) {
            //     OutPutError("KS_readEcatDataObj Error", error);
            //     return error;
            // }
            // for (int i = 0; i < data_size; i++) {
            //     printf("0x20A2 data[%d]: %d\n", i, read_sdo_data.data[i]);
            // }



            auto start_time = std::chrono::high_resolution_clock::now();
            auto error = KS_readEcatDataObj(slave_handle, 0x4001, 1, &read_sdo_data, &data_size, KSF_SDO | KSF_COMPLETE_ACCESS);
            // 记录结束时间
            auto end_time = std::chrono::high_resolution_clock::now();
            // 计算执行时间
            auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time).count();
            // 输出执行时间
            printf("read 0x40A0 ,length: %d \n", data_size);
            printf("KS_readEcatDataObj 0x40A0 Execution Time: %lld microseconds\n", duration);
            if (error != KS_OK) {
                OutPutError("KS_readEcatDataObj 0x4001 Error", error);
                return error;
            }
            for (int i = 0; i < 200; i++) {
                printf("0x4001 data[%d]: %d ; ", i, read_sdo_data[i]);
            }
            for (int i = 0; i < 200; i++) {
                read_sdo_data[i] = 0;
            }
            start_time = std::chrono::high_resolution_clock::now();
            error = KS_readEcatDataObj(slave_handle, 0x3001, 1, &read_sdo_data, &data_size, KSF_SDO | KSF_COMPLETE_ACCESS);
            // 记录结束时间
            end_time = std::chrono::high_resolution_clock::now();
            // 计算执行时间
            duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time).count();
            // 输出执行时间
            printf("read 0x3001 ,length: %d \n", data_size);
            printf("KS_readEcatDataObj 0x3001 Execution Time: %lld microseconds\n", duration);
            if (error != KS_OK) {
                OutPutError("KS_readEcatDataObj 0x40A0 Error", error);
                return error;
            }
            for (int i = 0; i < 200; i++) {
                printf("0x3001 data[%d]: %d ; ", i, read_sdo_data[i]);
            }
        }
    }

    printf("SDO_GetData Success \n");
    return KS_OK;
}
